/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "fc_sensors.h"
#include "filter.h"
#include "fc_error.h"

#define DBG_TAG "gyro"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

biquadFilter_t gyro_lpf1[3];
biquadFilter_t gyro_lpf2[3];

bool gyro_lpf_init = false;
bool gyro_use_lpf = true;
void gyro_filter_init(void)
{
    if(ZeroSave()->gyro_cutoff == 0)    {
        ZeroSave()->gyro_cutoff = GYRO_LPF_CUTOFF;
    }
    if(ZeroSave()->gyro_cutoff > 500)    {
        ZeroSave()->gyro_cutoff = 500;
    }
    LOG_I("gyro lpf cutoff: %dHz\n", ZeroSave()->gyro_cutoff);

    for (int i = 0; i < 3; i++)
    {
        biquadFilterInitLPF(&gyro_lpf1[i], ZeroSave()->gyro_cutoff, 1000.0f);
        // biquadFilterInitLPF(&gyro_lpf2[i], 50.0f, 1000.0f);
    }
}
void GyroStartCalibration(void)
{
    fc_sensor_data.gyro.gyro_offset.isdatacalibrated = false;
    fc_sensor_data.gyro.gyro_offset.samplesize = ACC_GYRO_CALIBRATE_SAMPLE;
    FC_ERROR_SET(FC_IMU_CALIBRATING);
}
void sensordata_save_gyro_bias(gyro_t *gyro)
{
    ZeroSave()->gyrooffset.x = gyro->gyro_offset.zero.x;
    ZeroSave()->gyrooffset.y = gyro->gyro_offset.zero.y;
    ZeroSave()->gyrooffset.z = gyro->gyro_offset.zero.z;

    zino_config_save_all();
}
void sensordata_get_save_gyro_bias(gyro_t *gyro)
{
    gyro->gyro_offset.zero.x = ZeroSave()->gyrooffset.x;
    gyro->gyro_offset.zero.y = ZeroSave()->gyrooffset.y;
    gyro->gyro_offset.zero.z = ZeroSave()->gyrooffset.z;
}
void sensordata_gyro_calibrate(gyro_t *gyro)
{
    if (gyro->gyro_offset.samplesize > 0)
    {
        if (gyro->gyro_offset.samplesize == ACC_GYRO_CALIBRATE_SAMPLE)
        {
            gyro->gyro_offset.zero.x = 0;
            gyro->gyro_offset.zero.y = 0;
            gyro->gyro_offset.zero.z = 0;
        }

        gyro->gyro_offset.zero.x += gyro->gyroRaw.x;
        gyro->gyro_offset.zero.y += gyro->gyroRaw.y;
        gyro->gyro_offset.zero.z += gyro->gyroRaw.z;

        if (gyro->gyro_offset.samplesize == 1)
        {
            gyro->gyro_offset.zero.x /= ACC_GYRO_CALIBRATE_SAMPLE;
            gyro->gyro_offset.zero.y /= ACC_GYRO_CALIBRATE_SAMPLE;
            gyro->gyro_offset.zero.z /= ACC_GYRO_CALIBRATE_SAMPLE;
            gyro->gyro_offset.isdatacalibrated = true;

            ZeroSave()->gyrooffset.x = gyro->gyro_offset.zero.x;
            ZeroSave()->gyrooffset.y = gyro->gyro_offset.zero.y;
            ZeroSave()->gyrooffset.z = gyro->gyro_offset.zero.z;
            ZeroSave()->gyro_calibrated = true;
            ZeroSave()->flag = ZERO_SAVED_FLAG;
            zino_config_save_all();
            FC_ERROR_RESET(FC_IMU_CALIBRATING);
        }
        gyro->gyro_offset.samplesize--;
        // rt_kprintf("gyro calibrate %d\r\n", gyro->gyro_offset.samplesize);
    }
}
void sensor_gyro_process(struct rt_sensor_data *sensor)
{
    if (sensor->type == RT_SENSOR_CLASS_GYRO)
    {
        gyro_t *gyro = &fc_sensor_data.gyro;

        gyro->gyroRaw.x = sensor->data.gyro.x;
        gyro->gyroRaw.y = sensor->data.gyro.y;
        gyro->gyroRaw.z = sensor->data.gyro.z;
        if (!ZeroSave()->gyro_calibrated || !gyro->gyro_offset.isdatacalibrated)
        {
            if (!ZeroSave()->gyro_calibrated)
            {
                ZeroSave()->gyrooffset.x = 0;
                ZeroSave()->gyrooffset.y = 0;
                ZeroSave()->gyrooffset.z = 0;
            }
            if (gyro->gyro_offset.samplesize == 0)
            {
                GyroStartCalibration();
            }
            sensordata_gyro_calibrate(gyro);
        }
        gyro->gyrodata.x = (float)(gyro->gyroRaw.x - ZeroSave()->gyrooffset.x) / 1000.f;
        gyro->gyrodata.y = (float)(gyro->gyroRaw.y - ZeroSave()->gyrooffset.y) / 1000.f;
        gyro->gyrodata.z = (float)(gyro->gyroRaw.z - ZeroSave()->gyrooffset.z) / 1000.f;

        if (!gyro_lpf_init)
        {
            gyro_filter_init();
            gyro_lpf_init = true;
        }
        for (uint8_t i = 0; i < 3; i++)
        {
            if(gyro_use_lpf)
            {
                gyro->gyrolpf.axis[i] = biquadFilterApply(&gyro_lpf1[i], gyro->gyrodata.axis[i]);
                // gyro->gyrolpf.axis[i] = biquadFilterApply(&gyro_lpf2[i], gyro->gyrolpf.axis[i]);
            }
            else
            {
                gyro->gyrolpf.axis[i] = gyro->gyrodata.axis[i];
            }
        }
    }
}
